/*
 Copyright (C) 2008 PAUCARD Charlie

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License along
with this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/

#include "WM.h"
using namespace std;

/**
 *Default constructor
 *@param actAcc
 *      Activate or not the accelerometer
 *@param actIR
 *      Activate or not the IR
 *@param nbWiiMotes
 *      Number of wii remotes to connect at MAX
 */
WM::WM(bool actAcc, bool actIR, int nbWiiMotes)
{
    //Initialization
    M_NB_WIIMOTES = nbWiiMotes; //Set the max number of wiimotes to 1
    m_wiimotes = wiiuse_init(M_NB_WIIMOTES); //Initialize the wiimotes
    
    //Find wiimotes
    int wiimotes_found = wiiuse_find(m_wiimotes, M_NB_WIIMOTES, M_DEFAULT_TIMEOUT); //Starts looking for wiimotes i, discovery mode, default time out : 5
    if(!wiimotes_found)
        cout << "[ERR]1 : No wiimotes found, please try again." << endl;
    
    //Connect wiimiotes
    int wiimotes_connected = wiiuse_connect(m_wiimotes, M_NB_WIIMOTES); //Connect the found wiimotes
    if(!wiimotes_connected)
        cout << "[ERR]2 : Unable to connect to wiimotes." << endl;
    
    setAcc(actAcc); //Activate accelerometer and IR
    setIR(actIR); 
}

//Destructor
WM::~WM()
{
    wiiuse_cleanup(m_wiimotes, M_NB_WIIMOTES);
}

/**
 *Function to activate/deactivate the accelerometer on a specific wiimote
 *@param choice
 *      Activate/Deactivate
 *@param wiimoteNb
 *      The wiimote to operate on (starts with 0)
 */
void WM::setAcc(bool choice, int wiimoteNb)
{
    if(choice)
        wiiuse_motion_sensing(m_wiimotes[wiimoteNb], 1); //Activate
    else
        wiiuse_motion_sensing(m_wiimotes[wiimoteNb], 0); //Deactivate
}
/**
 *Function to activate/deactivate the accelerometer on a all wiimotes
 *@param choice
 *      Activate/Deactivate
 */
void WM::setAcc(bool choice)
{
    for(int i=0; i<M_NB_WIIMOTES; i++)
        setAcc(choice, i);
}
/**
 *Function to activate/deactivate the IR on a specific wiimote
 *@param choice
 *      Activate/Deactivate
 *@param wiimoteNb
 *      The wiimote to operate on (starts with 0)
 */
void WM::setIR(bool choice, int wiimoteNb)
{
    if(choice)
        wiiuse_set_ir(m_wiimotes[wiimoteNb], 1); //Activate
    else
        wiiuse_set_ir(m_wiimotes[wiimoteNb], 0); //Deactivate
}
/**
 *Function to activate/deactivate the IR on a all wiimotes
 *@param choice
 *      Activate/Deactivate
 */
void WM::setIR(bool choice)
{
     for(int i=0; i<M_NB_WIIMOTES; i++)
        setIR(choice, i);
}

/**
 *Function to determine whether an event happened
 *@return True if event happened, false otherwise
 */
bool WM::eventHappened()
{
    if (wiiuse_poll(m_wiimotes, M_NB_WIIMOTES)) //If an event happened, tell the user
        return true;
    else
        return false;
}
/**
 *Function to return the event tha was thrown
 *@return 
 *      The return values are in a vector<double> of this type : 
 *      [0] -> Type of event
 *      [1] -> What happened in event (-1.0 if nothing happened)
 *      [2] -> Number of return values (-1.0 if no vamues were returned)
 *      [3-x] -> Return values
 *@param WiimoteNb
 *      The number of the wiimote to get events from
 */
vector<double> WM::getEvent(int WiimoteNb)
{
    if(WiimoteNb > M_NB_WIIMOTES)
        WiimoteNb = 0; //If the wiimote number is too big, use the first wiimote
    
    vector<double> return_value;
    switch (m_wiimotes[WiimoteNb]->event) 
    {
            case WIIUSE_EVENT:
                //An event occured
                return_value.resize(handleEvent(WiimoteNb).size());
                return_value = handleEvent(WiimoteNb);
                break;
                
            case WIIUSE_DISCONNECT:
            case WIIUSE_UNEXPECTED_DISCONNECT:
                //We disconnected. Send error to the user
                return_value.push_back(WM_DISCONNECTED);
                return_value.push_back(-1.0);
                break;

            case WIIUSE_STATUS:
            case WIIUSE_READ_DATA:
            case WIIUSE_NUNCHUK_INSERTED:
            case WIIUSE_CLASSIC_CTRL_INSERTED:
            case WIIUSE_GUITAR_HERO_3_CTRL_INSERTED:
            case WIIUSE_NUNCHUK_REMOVED:
            case WIIUSE_CLASSIC_CTRL_REMOVED:
            case WIIUSE_GUITAR_HERO_3_CTRL_REMOVED:
            default:
                return_value.push_back(WM_UNKNOWN);
                return_value.push_back(-1.0);
                break;
    }
    return return_value;
}
/**
 *Function to return the event that was thrown
 *@return 
 *      The return values are in a vector<double> of this type : 
 *      [0] -> Type of event
 *      [1] -> What happened in event (-1.0 if nothing happened)
 *@param WiimoteNb
 *      The number of the wiimote to get events from
 */
vector<double> WM::handleEvent(int wiimoteNb)
{
    //Handle the event
     vector<double> return_value;
     
     return_value.push_back(WM_EVENT);
     
     //Buttons
    if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_A))
    { return_value.push_back(WM_BUTTON_A); return_value.push_back(-1.0); }//Push back the button and the numbdr of return values value (a button has no value, so -1.0)
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_B))
    {    return_value.push_back(WM_BUTTON_B); return_value.push_back(-1.0); }
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_UP))
    {    return_value.push_back(WM_BUTTON_UP); return_value.push_back(-1.0); }
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_DOWN))
    {   return_value.push_back(WM_BUTTON_DOWN); return_value.push_back(-1.0); }
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_LEFT))
    {   return_value.push_back(WM_BUTTON_LEFT); return_value.push_back(-1.0); }
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_RIGHT))
    {   return_value.push_back(WM_BUTTON_RIGHT); return_value.push_back(-1.0); }
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_MINUS))
    {   return_value.push_back(WM_BUTTON_MINUS); return_value.push_back(-1.0); }
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_PLUS))
    {   return_value.push_back(WM_BUTTON_PLUS); return_value.push_back(-1.0); }
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_ONE))
    {   return_value.push_back(WM_BUTTON_ONE); return_value.push_back(-1.0); }
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_TWO))
    {   return_value.push_back(WM_BUTTON_TWO); return_value.push_back(-1.0); }
    else if (IS_PRESSED(m_wiimotes[wiimoteNb], WIIMOTE_BUTTON_HOME))
    {   return_value.push_back(WM_BUTTON_HOME); return_value.push_back(-1.0); }
    else
    { return_value.push_back(WM_EVENT_OTHER); return_value.push_back(-1.0); }
    return return_value; 
}

/**
 *Function to return the pithc, roll and yaw of the Wiimote
 *@return 
 *      The return values are in a vector<double> of this type : 
 *      [0] -> Type of event
 *      [1] -> What happened in event (-1.0 if nothing happened)
 *      [2] -> Number of return values (-1.0 if no vamues were returned)
 *      [3-x] -> Return values
 *@param WiimoteNb
 *      The number of the wiimote to get events from
 */
vector<double> WM::getAccData(int wiimoteNb)
{
    vector<double> return_value;
    //If the accelerometer is on, then return the angles
    if (WIIUSE_USING_ACC(m_wiimotes[wiimoteNb])) {
            return_value.push_back(WM_ACC);
            return_value.push_back(3.0); //We have 5 return values here
            return_value.push_back(m_wiimotes[wiimoteNb]->orient.roll); //The eventually smoother roll
            return_value.push_back(m_wiimotes[wiimoteNb]->orient.pitch); //Same but pitch
            return_value.push_back(m_wiimotes[wiimoteNb]->orient.yaw); //Only works if IR is enabled
    }
    else
    {
        return_value.push_back(WM_ACC_DISABLED);
        return_value.push_back(-1.0);
    }
    return return_value;
}

/**
 *Function to return the event the coords of tthe wiimote on the screen
 *@return 
 *      The return values are in a vector<double> of this type : 
 *      [0] -> Type of event
 *      [1] -> What happened in event (-1.0 if nothing happened)
 *      [2] -> Number of return values (-1.0 if no vamues were returned)
 *      [3-x] -> Return values
 *@param WiimoteNb
 *      The number of the wiimote to get events from
 */
vector<double> WM::getIRData(int wiimoteNb) //IR Data
{
     vector<double> return_value;
    if (WIIUSE_USING_IR(m_wiimotes[wiimoteNb]))
    {
            return_value.push_back(WM_IR);
            return_value.push_back(3.0);
            return_value.push_back((double)m_wiimotes[wiimoteNb]->ir.x); //return the coords
            return_value.push_back((double)m_wiimotes[wiimoteNb]->ir.y);
            return_value.push_back((double)m_wiimotes[wiimoteNb]->ir.z);
    }
    else
    {
        return_value.push_back(WM_IR_DISABLED);
        return_value.push_back(-1.0);
    } 
    return return_value;
}
/**
 *Activate the rumble on all wiimotes
 *@param choice
 *      Activate/Deactivate Rumble
 */
void WM::setRumble(bool choice)
{
    for(int i=0; i<M_NB_WIIMOTES; i++)
        setRumble(i, choice);
}
/**
 *Activate the rumble on all wiimotes
 *@param choice
 *      Activate/Deactivate Rumble
 *@param wiimoteNb
 *      The number of the wiimote to operate on
 */
void WM::setRumble(int wiimoteNb, bool choice)
{
    if(choice)
        wiiuse_rumble(m_wiimotes[wiimoteNb], 1);
    else
        wiiuse_rumble(m_wiimotes[wiimoteNb], 0);
}
/**
 *Activate the rumble on all wiimotes
 *@param led
 *      the number of the LED to activate [1..4]
 *@param wiimoteNb
 *      The number of the wiimote to operate on
 */
void WM::setLED(int wiimoteNb, int led)
{
    switch(led)
    {
        case 1:
            wiiuse_set_leds(m_wiimotes[wiimoteNb], WIIMOTE_LED_1);
            break;
        case 2:
            wiiuse_set_leds(m_wiimotes[wiimoteNb], WIIMOTE_LED_2);
            break;
        case 3:
            wiiuse_set_leds(m_wiimotes[wiimoteNb], WIIMOTE_LED_3);
            break;
        case 4:
            wiiuse_set_leds(m_wiimotes[wiimoteNb], WIIMOTE_LED_4);
            break;
    }
}

bool WM::getIR(int wiimoteNb)
{
    if(WIIUSE_USING_IR(m_wiimotes[wiimoteNb]))
        return true;
    else
        return false;
}
bool WM::getAcc(int wiimoteNb)
{
    if(WIIUSE_USING_ACC(m_wiimotes[wiimoteNb]))
        return true;
    else
        return false;
}
int WM::getBattery(int wiimoteNb)
{
    return (int)((m_wiimotes[wiimoteNb]->battery_level)*100);
}